import optimizationalgorithm as al
import cv2
import numpy as np
from scipy import optimize
from PIL import Image
import matplotlib.pyplot as plt
import glob

# 双目相机的参数
P= (  [[1.55545032e+03,6.62505387e+02,1.55546765e+03,5.37266926e+02,
        1.55492456e+03, 6.17343953e+02,1.55441833e+03 ,4.81399857e+02,
        0.999892485721600,-0.00134230600479356,0.0146018907018903,
        0.00123473877089462,0.999972055373835,0.00737318734302526,
        -0.0146113797311583,-0.00735436509953058,0.999866201497148,
        -138.421087392432,0.0340411468217300, 0.924604888200920]])

R_lr = np.array([[0.999892485721600,-0.00134230600479356,0.0146018907018903],
                 [0.00123473877089462,0.999972055373835,0.00737318734302526],
                 [-0.0146113797311583,-0.00735436509953058,0.999866201497148]])
T_lr = np.array([[-138.421087392432],
                 [0.0340411468217300],
                 [0.924604888200920]])




# 读取图片
imagesL = glob.glob('ALL\\*.bmp')
imagesR = glob.glob('ARR\\*.bmp')
l0 = []
r0 = []
l1 = []
r1 = []
l2 = []
r2 = []
l3 = []
r3 = []




for imgL in imagesL:
    grayImageL = cv2.imread(imgL,0)
    imgR = imgL.replace('ALL','ARR')
    imgR = imgR.replace('LL','RR')
    grayImageR = cv2.imread(imgR,0)


    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    retL, cornersL = cv2.findChessboardCorners(grayImageL, (15, 11), None)
    retR, cornersR = cv2.findChessboardCorners(grayImageR, (15, 11), None)
    cornersLO = cv2.cornerSubPix(grayImageL, cornersL, (15, 11), (-1, -1), criteria)
    cornersRO = cv2.cornerSubPix(grayImageR, cornersR, (15, 11), (-1, -1), criteria)


    # 棋盘格的四个角点坐标
    i=0
    l0.append([cornersLO[i][0][0], cornersLO[i][0][1]])
    r0.append([cornersRO[i][0][0], cornersRO[i][0][1]])

    l1.append([cornersLO[i+14][0][0], cornersLO[i+14][0][1]])
    r1.append([cornersRO[i+14][0][0], cornersRO[i+14][0][1]])

    l2.append([cornersLO[i+150][0][0], cornersLO[i+150][0][1]])
    r2.append([cornersRO[i+150][0][0], cornersRO[i+150][0][1]])

    l3.append([cornersLO[i+164][0][0], cornersLO[i+164][0][1]])
    r3.append([cornersRO[i+164][0][0], cornersRO[i+164][0][1]])

sunmf=0
f=[]
# 优化目标函数的建立：6个距离约束+8个直线方程+6个旋转矩阵约束
def func(P):
    global sumf
    Fend =[]

    for i in range(0,12):
        Ll0 = al.calLeftLineFromCameramtx(P, l0[i], 0.0053)
        Lr0 = al.calRightLineFromCameramtx(P, r0[i], 0.0053)

        Ll1 = al.calLeftLineFromCameramtx(P, l1[i], 0.0053)
        Lr1 = al.calRightLineFromCameramtx(P, r1[i], 0.0053)

        Ll2 = al.calLeftLineFromCameramtx(P, l2[i], 0.0053)
        Lr2 = al.calRightLineFromCameramtx(P, r2[i], 0.0053)

        Ll3 = al.calLeftLineFromCameramtx(P, l3[i], 0.0053)
        Lr3 = al.calRightLineFromCameramtx(P, r3[i], 0.0053)

        w0 = al.calPointCoordinateFromMatrixandRT(P, l0[i][0], l0[i][1], r0[i][0], r0[i][1])
        w1 = al.calPointCoordinateFromMatrixandRT(P, l1[i][0], l1[i][1], r1[i][0], r1[i][1])
        w2 = al.calPointCoordinateFromMatrixandRT(P, l2[i][0], l2[i][1], r2[i][0], r2[i][1])
        w3 = al.calPointCoordinateFromMatrixandRT(P, l3[i][0], l3[i][1], r3[i][0], r3[i][1])

        wr0 = np.dot(R_lr,(w0+T_lr))
        wr1 = np.dot(R_lr,(w1+T_lr))
        wr2 = np.dot(R_lr,(w2+T_lr))
        wr3 = np.dot(R_lr,(w3+T_lr))

        F =[np.sqrt((w0[0] - w3[0]) ** 2 + (w0[1] - w3[1]) ** 2 + (w0[2] - w3[2]) ** 2)-(np.sqrt(420**2+300**2)),
            np.sqrt((w1[0] - w2[0]) ** 2 + (w1[1] - w2[1]) ** 2 + (w1[2] - w2[2]) ** 2)-(np.sqrt(420**2+300**2)),

            (np.sqrt((w0[0] - w1[0]) ** 2 + (w0[1] - w1[1]) ** 2 + (w0[2] - w1[2]) ** 2)) - (14*30),
            (np.sqrt((w2[0] - w3[0]) ** 2 + (w2[1] - w3[1]) ** 2 + (w2[2] - w3[2]) ** 2)) - (14*30),

            (np.sqrt((w0[0] - w2[0]) ** 2 + (w0[1] - w2[1]) ** 2 + (w0[2] - w2[2]) ** 2)) - (10*30),
            (np.sqrt((w1[0] - w3[0]) ** 2 + (w1[1] - w3[1]) ** 2 + (w1[2] - w3[2]) ** 2)) - (10*30),


            w0[0] * Ll0[1] - w0[1] * Ll0[0],
            w0[0] * Ll0[2] - w0[2] * Ll0[0],
            w0[1] * Ll0[2] - w0[2] * Ll0[1],
            wr0[0] * Lr0[1] - wr0[1] * Lr0[0],
            wr0[0] * Lr0[2] - wr0[2] * Lr0[0],
            wr0[1] * Lr0[2] - wr0[2] * Lr0[1],

            w1[0] * Ll1[1] - w1[1] * Ll1[0],
            w1[0] * Ll1[2] - w1[2] * Ll1[0],
            w1[1] * Ll1[2] - w1[2] * Ll1[1],
            wr1[0] * Lr1[1] - wr1[1] * Lr1[0],
            wr1[0] * Lr1[2] - wr1[2] * Lr1[0],
            wr1[1] * Lr1[2] - wr1[2] * Lr1[1],

            w2[0] * Ll2[1] - w2[1] * Ll2[0],
            w2[0] * Ll2[2] - w2[2] * Ll2[0],
            w2[1] * Ll2[2] - w2[2] * Ll2[1],
            wr2[0] * Lr2[1] - wr2[1] * Lr2[0],
            wr2[0] * Lr2[2] - wr2[2] * Lr2[0],
            wr2[1] * Lr2[2] - wr2[2] * Lr2[1],

            w3[0] * Ll3[1] - w3[1] * Ll3[0],
            w3[0] * Ll3[2] - w3[2] * Ll3[0],
            w3[1] * Ll3[2] - w3[2] * Ll3[1],
            wr3[0] * Lr3[1] - wr3[1] * Lr3[0],
            wr3[0] * Lr3[2] - wr3[2] * Lr3[0],
            wr3[1] * Lr3[2] - wr3[2] * Lr3[1],

            np.array(((P[8]) ** 2 +(P[11]) ** 2 +  (P[14]) ** 2)-1).reshape(1),
            np.array(((P[9]) ** 2 + (P[12]) ** 2 + (P[15]) ** 2)-1).reshape(1),
            np.array(((P[10]) ** 2 + (P[13]) ** 2 + (P[16]) ** 2)-1).reshape(1),
            np.array((P[8] * P[9]) + (P[11] * P[12]) + (P[14] * P[15]) ).reshape(1),
            np.array((P[8] * P[10]) + (P[11] * P[13]) + (P[14] * P[16])).reshape(1),
            np.array((P[9] * P[10]) + (P[12] * P[13]) + (P[15] * P[16])).reshape(1)
            ]


        Fend.append(F)

    L =  np.array(Fend).flatten()
    L= np.array(L).reshape(-1)
    sumf = np.sum(Fend)
    print(sumf)
    return L










sol = optimize.root(func, P, jac=None, method='lm')


print('P', sol.x)




